#!/usr/bin/env python

# Copyright FYT 邹承甫 2022

import roslib
import rospy
from std_msgs.msg import Header
from std_msgs.msg import Float32
from sensor_msgs.msg import JointState
from rm_interfaces.srv import *
from rm_interfaces.msg import SerialSendMsg
from rm_interfaces.msg import SerialReceiveMsg

def serial_send_cb(msg):
    pass

def main():
  rospy.init_node("serial_node", anonymous=True)
  pub = rospy.Publisher("/serial_receive", data_class=SerialReceiveMsg, queue_size=10)
  joint_state_pub = rospy.Publisher("/joint_states", data_class=JointState, queue_size=1)
  sub = rospy.Subscriber("/serial_sendmsg", SerialSendMsg, serial_send_cb, queue_size=1)
  mode = 0
  first_start = True
  rate = rospy.Rate(100)
  while(not rospy.is_shutdown()):
    mode_now = rospy.get_param("/serial/mode", default=0)
    joint_state = JointState()
    joint_state.header.stamp = rospy.Time.now()
    joint_state.name.append("joint_yaw")
    joint_state.position.append(0)
    joint_state.name.append("joint_pitch")
    joint_state.position.append(0)
    joint_state_pub.publish(joint_state)
    if mode != mode_now or first_start:
        rospy.wait_for_service('/detector/set_mode')
        try:
            set_mode = rospy.ServiceProxy('/detector/set_mode', SetMode)
            resp = set_mode(mode_now)
            if resp:
                mode = mode_now
                first_start = False
        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)
    serial_msg = SerialReceiveMsg()
    serial_msg.header = joint_state.header
    serial_msg.bullet_speed = 25.0
    serial_msg.yaw = 0
    serial_msg.pitch = 0
    pub.publish(serial_msg)
    rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
